#include "kalman_filter.hpp"

// 1维卡尔曼滤波器初始化实现
void KalmanFilter::kalman1Init(Kalman1State* state, float init_x, float init_p) 
{
    state->x = init_x;
    state->p = init_p;
    state->A = 1.0f;  // 状态转移矩阵默认值
    state->H = 1.0f;  // 测量矩阵默认值
    state->q = 0.01f; // 过程噪声协方差（可根据场景调整）
    state->r = 0.1f;  // 测量噪声协方差（可根据场景调整）
}

// 1维卡尔曼滤波计算实现
float KalmanFilter::kalman1Filter(Kalman1State* state, float z_measure) 
{
    // 预测阶段：计算先验估计
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;

    // 更新阶段：计算卡尔曼增益并修正估计
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x += state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

// 2维卡尔曼滤波器初始化实现
void KalmanFilter::kalman2Init(Kalman2State* state, float* init_x, float (*init_p)[2]) 
{
    // 初始化状态值
    state->x[0] = init_x[0];
    state->x[1] = init_x[1];

    // 初始化估计误差协方差矩阵
    state->p[0][0] = init_p[0][0];
    state->p[0][1] = init_p[0][1];
    state->p[1][0] = init_p[1][0];
    state->p[1][1] = init_p[1][1];

    // 初始化状态转移矩阵（默认匀速运动模型）
    state->A[0][0] = 1.0f;
    state->A[0][1] = 0.1f;  // 时间间隔相关，可调整
    state->A[1][0] = 0.0f;
    state->A[1][1] = 1.0f;

    // 初始化测量矩阵
    state->H[0] = 1.0f;
    state->H[1] = 0.0f;

    // 初始化噪声协方差
    state->q[0] = 1e-7f;  // 过程噪声（角度）
    state->q[1] = 1e-7f;  // 过程噪声（角度变化率）
    state->r = 1e-7f;     // 测量噪声
}

// 2维卡尔曼滤波计算实现
float KalmanFilter::kalman2Filter(Kalman2State* state, float z_measure) 
{
    float temp0 = 0.0f;
    float temp1 = 0.0f;
    float temp = 0.0f;

    // 预测阶段：计算先验状态和协方差
    state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
    state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];

    // 更新先验估计误差协方差矩阵
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
    state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
    state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
    state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

    // 更新阶段：计算卡尔曼增益
    temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
    temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
    temp = state->r + state->H[0] * temp0 + state->H[1] * temp1;
    state->gain[0] = temp0 / temp;
    state->gain[1] = temp1 / temp;

    // 修正状态估计
    temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];
    state->x[0] += state->gain[0] * (z_measure - temp);
    state->x[1] += state->gain[1] * (z_measure - temp);

    // 更新后验估计误差协方差矩阵
    state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
    state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
    state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
    state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

    return state->x[0];
}